
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>

#include "pca9685.h"

#include "mqtt.h"
#include "posix_sockets.h"

typedef struct {
    unsigned char head;
    unsigned char type;
    unsigned char size;
    unsigned char body[255];
} msg_content_t;

msg_content_t my_content = {0, 0, 0, {0}};

/**
 * @brief The function that would be called whenever a PUBLISH is received.
 *
 * @note This function is not used in this example.
 */
void publish_callback(void** unused, struct mqtt_response_publish *published);

/**
 * @brief The client's refresher. This function triggers back-end routines to
 *        handle ingress/egress traffic to the broker.
 *
 * @note All this function needs to do is call \ref __mqtt_recv and
 *       \ref __mqtt_send every so often. I've picked 100 ms meaning that
 *       client ingress/egress traffic will be handled every 100 ms.
 */
void* client_refresher(void* client);

/**
 * @brief Safelty closes the \p sockfd and cancels the \p client_daemon before \c exit.
 */
void exit_example(int status, int sockfd, pthread_t *client_daemon);

void pca9685_init(void)
{

}

/**
 * A simple program to that publishes the current time whenever ENTER is pressed.
 */
int main(int argc, const char *argv[])
{
    const char* addr;
    const char* port;
    const char* topic;

    /* get address (argv[1] if present) */
    if (argc > 1) {
        addr = argv[1];
    } else {
        addr = "120.76.46.85";
    }

    /* get port number (argv[2] if present) */
    if (argc > 2) {
        port = argv[2];
    } else {
        port = "1883";
    }

    /* get the topic name to publish */
    if (argc > 3) {
        topic = argv[3];
    } else {
        topic = "/test/";
    }

    /* open the non-blocking TCP socket (connecting to the broker) */
    int sockfd = open_nb_socket(addr, port);

    if (sockfd == -1) {
        perror("Failed to open socket: ");
        exit_example(EXIT_FAILURE, sockfd, NULL);
    }

    /* setup a client */
    struct mqtt_client client;
    uint8_t sendbuf[2048]; /* sendbuf should be large enough to hold multiple whole mqtt messages */
    uint8_t recvbuf[1024]; /* recvbuf should be large enough any whole mqtt message expected to be received */
    mqtt_init(&client, sockfd, sendbuf, sizeof(sendbuf), recvbuf, sizeof(recvbuf), publish_callback);
    /* Create an anonymous session */
    const char* client_id = NULL;
    /* Ensure we have a clean session */
    uint8_t connect_flags = MQTT_CONNECT_CLEAN_SESSION;
    /* Send connection request to the broker. */
    mqtt_connect(&client, client_id, NULL, NULL, 0, NULL, NULL, connect_flags, 400);

    /* check that we don't have any errors */
    if (client.error != MQTT_OK) {
        fprintf(stderr, "error: %s\n", mqtt_error_str(client.error));
        exit_example(EXIT_FAILURE, sockfd, NULL);
    }

    /* start a thread to refresh the client (handle egress and ingree client traffic) */
    pthread_t client_daemon;
    if(pthread_create(&client_daemon, NULL, client_refresher, &client)) {
        fprintf(stderr, "Failed to start client daemon.\n");
        exit_example(EXIT_FAILURE, sockfd, NULL);

    }
	
	/* subscribe */
	mqtt_subscribe(&client, topic, 0);

    /* start publishing the time */
    printf("%s is ready to begin publishing the time.\n", argv[0]);
    printf("Press ENTER to publish the current time.\n");
    printf("Press CTRL-D (or any other key) to exit.\n\n");
    while(fgetc(stdin) == '\n') {
        /* get the current time */
        time_t timer;
        time(&timer);
        struct tm* tm_info = localtime(&timer);
        char timebuf[26];
        strftime(timebuf, 26, "%Y-%m-%d %H:%M:%S", tm_info);

        /* print a message */
        char application_message[256];
        snprintf(application_message, sizeof(application_message), "The time is %s", timebuf);
        printf("%s published : \"%s\"", argv[0], application_message);

        /* publish the time */
        mqtt_publish(&client, topic, application_message, strlen(application_message) + 1, MQTT_PUBLISH_QOS_0);

        /* check for errors */
        if (client.error != MQTT_OK) {
            fprintf(stderr, "error: %s\n", mqtt_error_str(client.error));
            exit_example(EXIT_FAILURE, sockfd, &client_daemon);
        }
    }

    /* disconnect */
    printf("\n%s disconnecting from %s\n", argv[0], addr);
    sleep(1);

    /* exit */
    exit_example(EXIT_SUCCESS, sockfd, &client_daemon);
}

void exit_example(int status, int sockfd, pthread_t *client_daemon)
{
    if (sockfd != -1) close(sockfd);
    if (client_daemon != NULL) pthread_cancel(*client_daemon);
    exit(status);
}

int hexstringtobyte(char *in, char *out) {
    int len = (int)strlen(in);
    char *str = (char *)malloc(len);

    memset(str, 0, len);
    memcpy(str, in, len);
    for (int i = 0; i < len; i += 2) {
        //小写转大写
        if (str[i] >= 'a' && str[i] <= 'f') str[i] = str[i] & ~0x20;
        if (str[i + 1] >= 'a' && str[i] <= 'f') str[i + 1] = str[i + 1] & ~0x20;
        //处理第前4位
        if (str[i] >= 'A' && str[i] <= 'F')
            out[i / 2] = (str[i] - 'A' + 10) << 4;
        else
            out[i / 2] = (str[i] & ~0x30) << 4;
        //处理后4位, 并组合起来
        if (str[i + 1] >= 'A' && str[i + 1] <= 'F')
            out[i / 2] |= (str[i + 1] - 'A' + 10);
        else
            out[i / 2] |= (str[i + 1] & ~0x30);
    }
    free(str);
    return 0;
}

void data_parse(char* in, short len)
{
    /*****************************
    head  type   len    data0 data1 data2 ...
    0x5A 0~0xff 0~0xff   -     -     -     -
    *****************************/
    short hex_size = (len/2)+1;
    char* hex_body = (char*) malloc(hex_size);

    if(in[0] == 53 && in[1] == 65)
    {
        hexstringtobyte(in, hex_body);
        hex_body[hex_size] = '\0';

        memcpy(&my_content, hex_body, hex_size);

        printf("%x, %x, %x\r\n", my_content.head, \
                                my_content.type, \
                                my_content.size);
        
        for(int i = 0; i < my_content.size; i++)
        {
            printf("body[%d]: %x\r\n", i, my_content.body[i]);
        }

        if(my_content.head == 0x5A)
        {
            switch(my_content.type)
            {
                case 0x01: // servo : {"msg":"5A0102085A"}
                {
                    if(my_content.size == 2)
                    {
                        pca9685_init_pwm();
        
                        pca9685_frequency_set(50);
                        
                        pca9685_sg90_angle_set(my_content.body[0], my_content.body[1]);
                    }
                }break;
                case 0x02: //dcmotor : {"msg":"5A0202015A"}
                {
                    if(my_content.size == 2)
                    {
                        pca9685_init_pwm();
        
                        pca9685_frequency_set(50);
                        
                        pca9685_dcmotor_duty_set(my_content.body[0], my_content.body[1]);
                    }
                }break;
            }
        }
    }
    else
    {
        printf("[ERROR] Format error!!\r\n");
    }

    free(hex_body);
}

void publish_callback(void** unused, struct mqtt_response_publish *published)
{
    /************************
     {"msg":"5A02052346889931"}
    ***************************/
    short pack_size = published->application_message_size - 10;

    short char_size = pack_size+1;
    char* char_body = (char*) malloc(char_size);

    char* topic_name = (char*) malloc(published->topic_name_size + 1);
    char* msg = (char*) malloc(published->application_message_size + 1);

    memcpy(topic_name, published->topic_name, published->topic_name_size);
    topic_name[published->topic_name_size] = '\0';

    memcpy(msg, published->application_message, published->application_message_size);
    msg[published->application_message_size] = '\0';

    // printf("Received publish('%s'): %s\n", topic_name, msg);

    /*printf("topic_len: %d, msg_len: %ld, content_len: %d\n", \
                             published->topic_name_size, \
                             published->application_message_size, \
                             pack_size);
    */

	memcpy(char_body, msg+8, pack_size);
    char_body[char_size] = '\0';
    // printf("content: %s\n", char_body);
   
    data_parse(char_body, pack_size);

    free(topic_name);
    free(msg);
    free(char_body);
}

void* client_refresher(void* client)
{
    while(1)
    {
        mqtt_sync((struct mqtt_client*) client);
        usleep(100000U);
    }
    return NULL;
}
